﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Oops.Xna.Framework
{
	/// <summary>
	/// A collection of items, added, allowing ordered removal.
	/// </summary>
	/// <typeparam name="T">The type of the items in the <see cref="PriorityQueue[T]"/>.</typeparam>
	public sealed class PriorityQueue<T>
	{
		private IComparer<T> comparer;
		private List<T> items = new List<T>();
		
		/// <summary>
		/// Initializes a new instance of the <see cref="PriorityQueue[T]"/> class.
		/// </summary>
		public PriorityQueue() : this(Comparer<T>.Default) { }

		/// <summary>
		/// Initializes a new instance of the <see cref="PriorityQueue[T]"/> class.
		/// </summary>
		/// <param name="comparer">The <see cref="IComparer[T]"/> implementation to use when comparing elements, or null reference to use the default comparer <see cref="Comparer[T].Default." />.</param>
		public PriorityQueue(IComparer<T> comparer)
		{
			// Create a comparer for the keys based on the provided comparer.
			this.comparer = comparer;
		}

		/// <summary>
		/// Gets the number of elements actually contained in the <see cref="PriorityQueue[T]"/>.
		/// </summary>
		public int Count
		{
			get { return this.items.Count; }
		}

		/// <summary>
		/// Removes all elements from the <see cref="PriorityQueue[T]"/>.
		/// </summary>
		public void Clear()
		{
			// Remove all the items and indices.
			this.items.Clear();
		}
		
		/// <summary>
		/// Inserts a value of type <typeparamref name="T"/> and prioritizes by the specified comparing routine.
		/// </summary>
		/// <param name="value">The value to add to the <see cref="PriorityQueue[T]"/>.</param>
		public void Enqueue(T value)
		{
			// The new item will have this index.
			int index = this.items.Count;
			
			// Add the item to collection and store it's index.
			this.items.Add(value);

			// Sort the item into place.
			while (index != 0)
			{
				int mid = (index - 1) >> 1;
				
				if (this.comparer.Compare(this.items[index], this.items[mid]) < 0)
				{
					this.Swap(index, mid);
					index = mid;
				}
				else
				{
					break;
				}
			}
		}
		
		/// <summary>
		/// Removes and returns the item with the highest priority from 
		/// the <see cref="PriorityQueue[T]"/>.
		/// </summary>
		/// <returns>
		/// The item removed from the <see cref="PriorityQueue[T]"/> with 
		/// the highest priority.
		/// </returns>
		public T Dequeue()
		{
			// Hold onto the first item.
			T result = this.items[0];

			// Move the item down to the bottom.
			this.Swap(0, this.items.Count - 1);

			// Remove the item and it's index.
			this.items.RemoveAt(this.items.Count - 1);
			
			// Make sure the collection is still sorted.
			int index = 0;
			do
			{
				int hold = index;
				int index0 = (index << 1) + 1;
				int index1 = (index << 1) + 2;

				if (index0 < this.items.Count && this.comparer.Compare(this.items[index], this.items[index0]) > 0)
				{
					index = index0;
				}
				if (index1 < this.items.Count && this.comparer.Compare(this.items[index], this.items[index1]) > 0)
				{
					index = index1;
				}

				if (index == hold)
				{
					break;
				}
				
				this.Swap(index, hold);
			}
			while (true);

			// Return the highest prioritized item.
			return result;
		}

		/// <summary>
		/// Returns the item with the highest priority from 
		/// the <see cref="PriorityQueue[T]"/> without removing it.
		/// </summary>
		/// <returns>
		/// The item removed from the <see cref="PriorityQueue[T]"/> with 
		/// the highest priority.
		/// </returns>
	    public T Peek()
		{
			return this.items[0];
		}

		/// <summary>
		/// Re-prioritizes the specified item due to a state change that affects its priority.
		/// </summary>
		/// <param name="value">The value to update.</param>
		public void Update(T value)
		{
			int index = this.items.IndexOf(value);

			// Hold onto the index.
			int localIndex = index;

			// Sort up the list.
			while (localIndex != 0)
			{
				int mid = (localIndex - 1) >> 1;

				if (this.comparer.Compare(this.items[localIndex], this.items[mid]) < 0)
				{
					this.Swap(localIndex, mid);
					localIndex = mid;
				}
				else
				{
					break;
				}
			}

			// The item is re-prioritized correctly.
			if (localIndex < index)
			{
				return;
			}

			// Sort down the list.
			do
			{
				int hold = localIndex;
				int index0 = (localIndex << 1) + 1;
				int index1 = (localIndex << 1) + 2;

				if (index0 < this.items.Count && this.comparer.Compare(this.items[localIndex], this.items[index0]) > 0)
				{
					localIndex = index0;
				}
				if (index1 < this.items.Count && this.comparer.Compare(this.items[localIndex], this.items[index1]) > 0)
				{
					localIndex = index1;
				}

				if (localIndex == hold)
				{
					break;
				}

				this.Swap(localIndex, hold);
			}
			while (true);
		}

		/// <summary>
		/// Exchanges the locations of the items in the <see cref="PriorityQueue[T]"/>.
		/// </summary>
		/// <param name="index0">The index of the first item.</param>
		/// <param name="index1">The index of the second item.</param>
		private void Swap(int index0, int index1)
		{
			T temp = this.items[index0];
			this.items[index0] = this.items[index1];
			this.items[index1] = temp;
		}
	}

	/// <summary>
	/// A collection of items, added and sorted with a key, allowing ordered removal.
	/// </summary>
	/// <typeparam name="TKey">The type of the keys in the <see cref="PriorityQueue[TKey, TValue]"/>.</typeparam>
	/// <typeparam name="TValue">The type of the values in the <see cref="PriorityQueue[TKey, TValue]"/>.</typeparam>
	public sealed class PriorityQueue<TKey, TValue>
	{
		#region Comparer Declaration

		/// <summary>
		/// Defines a method to compare items based on their key values.
		/// </summary>
		internal class Comparer : IComparer<KeyValuePair<TKey, TValue>>
		{
			private IComparer<TKey> _comparer;

			/// <summary>
			/// Initializes a new instance of the <see cref="Comparer"/> class.
			/// </summary>
			/// <param name="comparer"></param>
			internal Comparer(IComparer<TKey> comparer)
			{
				this._comparer = comparer;
			}

			#region IComparer<KeyValuePair<TKey, TValue>> Members

			int IComparer<KeyValuePair<TKey, TValue>>.Compare(KeyValuePair<TKey, TValue> x, KeyValuePair<TKey, TValue> y)
			{
				return this._comparer.Compare(x.Key, y.Key);
			}

			#endregion
		}

		#endregion

		private IComparer<KeyValuePair<TKey, TValue>> comparer;
		private List<KeyValuePair<TKey, TValue>> items = new List<KeyValuePair<TKey, TValue>>();
		private Dictionary<TValue, int> indices = new Dictionary<TValue, int>();

		/// <summary>
		/// Initializes a new instance of the <see cref="PriorityQueue[TKey, TValue]"/> class.
		/// </summary>
		public PriorityQueue() : this(Comparer<TKey>.Default) { }

		/// <summary>
		/// Initializes a new instance of the <see cref="PriorityQueue[TKey, TValue]"/> class.
		/// </summary>
		/// <param name="comparer">The <see cref="IComparer[TKey]"/> implementation to use when comparing elements, or null reference to use the default comparer <see cref="Comparer[T].Default." />.</param>
		public PriorityQueue(IComparer<TKey> comparer)
		{
			// Create a comparer for the keys based on the provided comparer.
			this.comparer = new Comparer(comparer);
		}

		/// <summary>
		/// Gets the number of elements actually contained in the <see cref="PriorityQueue[TKey, TValue]"/>.
		/// </summary>
		public int Count
		{
			get { return this.items.Count; }
		}

		/// <summary>
		/// Removes all elements from the <see cref="PriorityQueue[TKey, TValue]"/>.
		/// </summary>
		public void Clear()
		{
			// Remove all the items and indices.
			this.items.Clear();
			this.indices.Clear();
		}

		/// <summary>
		/// Inserts a value of type <typeparamref name="TValue"/> and prioritizes by a key of type <typeparamref name="TKey"/>.
		/// </summary>
		/// <param name="key">The key to prioritize the value by.</param>
		/// <param name="value">The value to add to the <see cref="PriorityQueue[TKey, TValue]"/>.</param>
		public void Enqueue(TKey key, TValue value)
		{
			// The new item will have this index.
			int index = this.items.Count;

			// Add the item to collection and store it's index.
			this.items.Add(new KeyValuePair<TKey, TValue>(key, value));
			this.indices.Add(value, index);

			// Sort the item into place.
			while (index != 0)
			{
				int mid = (index - 1) >> 1;

				if (this.comparer.Compare(this.items[index], this.items[mid]) < 0)
				{
					this.Swap(index, mid);
					index = mid;
				}
				else
				{
					break;
				}
			}
		}

		/// <summary>
		/// Removes and returns the item with the highest priority from 
		/// the <see cref="PriorityQueue[TKey, TValue]"/>.
		/// </summary>
		/// <returns>
		/// The item removed from the <see cref="PriorityQueue[TKey, TValue]"/> with 
		/// the highest priority.
		/// </returns>
		public TValue Dequeue()
		{
			// Hold onto the first item.
			KeyValuePair<TKey, TValue> result = this.items[0];

			// Move the item down to the bottom.
			this.Swap(0, this.items.Count - 1);

			// Remove the item and it's index.
			this.items.RemoveAt(this.items.Count - 1);
			this.indices.Remove(result.Value);

			// Make sure the collection is still sorted.
			int index = 0;
			do
			{
				int hold = index;
				int index0 = (index << 1) + 1;
				int index1 = (index << 1) + 2;

				if (index0 < this.items.Count && this.comparer.Compare(this.items[index], this.items[index0]) > 0)
				{
					index = index0;
				}
				if (index1 < this.items.Count && this.comparer.Compare(this.items[index], this.items[index1]) > 0)
				{
					index = index1;
				}

				if (index == hold)
				{
					break;
				}

				this.Swap(index, hold);
			}
			while (true);

			// Return the highest prioritized item.
			return result.Value;
		}

		/// <summary>
		/// Returns the item with the highest priority from 
		/// the <see cref="PriorityQueue[TKey, TValue]"/> without removing it.
		/// </summary>
		/// <returns>
		/// The item removed from the <see cref="PriorityQueue[TKey, TValue]"/> with 
		/// the highest priority.
		/// </returns>
		public TValue Peek()
		{
			return this.items[0].Value;
		}

		/// <summary>
		/// Updates the value's specified key and re-prioritizes.
		/// </summary>
		/// <param name="key">The new key value.</param>
		/// <param name="value">The value to update.</param>
		public void Update(TKey key, TValue value)
		{
			// Call overloaded method.
			this.Update(key, this.indices[value]);
		}

		/// <summary>
		/// Updates the value's specified key and re-prioritizes.
		/// </summary>
		/// <param name="key">The new key value.</param>
		/// <param name="index">The index of the value to update.</param>
		private void Update(TKey key, int index)
		{
			// Hold onto the index.
			int localIndex = index;

			// Update the item's key.
			this.items[index] = new KeyValuePair<TKey, TValue>(key, this.items[index].Value);

			// Sort up the list.
			while (localIndex != 0)
			{
				int mid = (localIndex - 1) >> 1;

				if (this.comparer.Compare(this.items[localIndex], this.items[mid]) < 0)
				{
					this.Swap(localIndex, mid);
					localIndex = mid;
				}
				else
				{
					break;
				}
			}

			// The item is re-prioritized correctly.
			if (localIndex < index)
			{
				return;
			}

			// Sort down the list.
			do
			{
				int hold = localIndex;
				int index0 = (localIndex << 1) + 1;
				int index1 = (localIndex << 1) + 2;

				if (index0 < this.items.Count && this.comparer.Compare(this.items[localIndex], this.items[index0]) > 0)
				{
					localIndex = index0;
				}
				if (index1 < this.items.Count && this.comparer.Compare(this.items[localIndex], this.items[index1]) > 0)
				{
					localIndex = index1;
				}

				if (localIndex == hold)
				{
					break;
				}

				this.Swap(localIndex, hold);
			}
			while (true);
		}

		/// <summary>
		/// Exchanges the locations of the items in the <see cref="PriorityQueue[TKey, TValue]"/>.
		/// </summary>
		/// <param name="index0">The index of the first item.</param>
		/// <param name="index1">The index of the second item.</param>
		private void Swap(int index0, int index1)
		{
			int index = this.indices[this.items[index0].Value];
			KeyValuePair<TKey, TValue> temp = this.items[index0];

			this.indices[this.items[index0].Value] = this.indices[this.items[index1].Value];
			this.items[index0] = this.items[index1];

			this.indices[this.items[index1].Value] = index;
			this.items[index1] = temp;
		}
	}
}
